using System;
using System.Collections.Generic;
using System.Threading;
using System.Globalization;
using Project3D.my3D.Structure;
using Project3D.my3D.File3D;
using Project3D.my3D.Base;
using Project3D.my3D.Geometric;

namespace Project3D.my3D
{

    public class myMesh
    {
        //The table containing vertices of this mesh.
        public List<myVertex> vertices = new List<myVertex>();

        //The table containing halfedges of this mesh.
        public List<myHalfedge> halfedges = new List<myHalfedge>();

        //The table containing faces of this mesh.
        public List<myFace> faces = new List<myFace>();



        public bool read(string filename)
        {
            return OBJ_File.read(filename, this);
        }

        public void print()
        {
        }

        public void write(string filename = "noname.obj")
        {
            OBJ_File.write(filename, this, false);
        }

        public void write_triangularmesh(string filename = "noname.obj")
        {
            OBJ_File.write(filename, this, true);
        }

        public void edgeSplit(myHalfedge e)
        {
            myVertex midVertex = new myVertex(myPoint3D.MidPoint(e.source.point, e.next.source.point));
            myHalfedge newHalfedge = new myHalfedge();

            this.vertices.Add(midVertex);
            this.halfedges.Add(newHalfedge);

            midVertex.originof_halfedge = newHalfedge;
            newHalfedge.source = midVertex;
            newHalfedge.adjacent_face = e.adjacent_face;

            newHalfedge.next = e.next;
            e.next.prev = newHalfedge;

            newHalfedge.prev = e;
            e.next = newHalfedge;

            if (e.twin != null)
            {
                myHalfedge newHalfedgeTwin = new myHalfedge();
                myHalfedge eTwin = e.twin;

                this.halfedges.Add(newHalfedgeTwin);

                newHalfedgeTwin.source = midVertex;
                newHalfedgeTwin.adjacent_face = eTwin.adjacent_face;

                newHalfedgeTwin.next = eTwin.next;
                eTwin.next.prev = newHalfedgeTwin;

                newHalfedgeTwin.prev = eTwin;
                eTwin.next = newHalfedgeTwin;

                e.twin = newHalfedgeTwin;
                newHalfedgeTwin.twin = e;

                newHalfedge.twin = eTwin;
                eTwin.twin = newHalfedge;

            }
        }

        public void faceSplit(myFace f)
        {
            List<myPoint3D> listOfPoint = new List<myPoint3D>();
            myHalfedge edge = f.adjacent_halfedge, nextEdge, firstEdge, secondEdge, twinFirstEdge;
            myHalfedge[] newHalfedges;
            myFace[] newFaces;
            myVertex centroid;
            myFace face;
            int i = 0;
            
            do
            {
                listOfPoint.Add(edge.source.point);
                edge = edge.next;
            } while (edge != f.adjacent_halfedge);

            centroid = new myVertex(myPoint3D.CentroidPoint(listOfPoint.ToArray()));
            this.vertices.Add(centroid);

            newHalfedges = new myHalfedge[listOfPoint.Count * 2];
            newFaces = new myFace[listOfPoint.Count];

            for (i = 0; i < listOfPoint.Count; i++)
            {
                newHalfedges[i] = new myHalfedge();
                newHalfedges[i + listOfPoint.Count] = new myHalfedge();
                newFaces[i] = new myFace();

                this.halfedges.Add(newHalfedges[i]);
                this.halfedges.Add(newHalfedges[i + listOfPoint.Count]);
                this.faces.Add(newFaces[i]);
            }

            centroid.originof_halfedge = newHalfedges[1];
            edge = f.adjacent_halfedge;
            i = 0;

            do
            {
                firstEdge = newHalfedges[i];
                secondEdge = newHalfedges[i + 1];
                face = newFaces[i / 2];
                nextEdge = edge.next;

                edge.next = firstEdge;
                edge.prev = secondEdge;

                firstEdge.next = secondEdge;
                firstEdge.prev = edge;

                secondEdge.next = edge;
                secondEdge.prev = firstEdge;

                face.adjacent_halfedge = edge;
                edge.adjacent_face = face;
                firstEdge.adjacent_face = face;
                secondEdge.adjacent_face = face;

                firstEdge.source = nextEdge.source;
                secondEdge.source = centroid;

                twinFirstEdge = newHalfedges[(i + 3) % newHalfedges.Length];
                firstEdge.twin = twinFirstEdge;
                twinFirstEdge.twin = firstEdge;

                edge = nextEdge;
                i += 2;
            } while (edge != f.adjacent_halfedge);

            this.faces.Remove(f);                    
        }

        public void triangulate()
        {
            myFace[] oldFaces = this.faces.ToArray();
            myFace newFace;
            myHalfedge newEdge, newEdge2;
            myHalfedge edge, nextEdge, twinEdge = null;
            myVertex firstvertex;

            foreach (myFace face in oldFaces)
                if (face.adjacent_halfedge != face.adjacent_halfedge.next.next.next)
                {
                    this.faces.Remove(face);

                    firstvertex = face.adjacent_halfedge.source;
                    edge = face.adjacent_halfedge;

                    do
                    {
                        newFace = new myFace();
                        newEdge = new myHalfedge();

                        this.halfedges.Add(newEdge);
                        this.faces.Add(newFace);

                        newFace.adjacent_halfedge = edge;
                        newEdge.adjacent_face = newFace;
                        edge.adjacent_face = newFace;

                        if (edge == face.adjacent_halfedge) // Frist new Face
                        {
                            nextEdge = edge.next.next;

                            edge.prev = newEdge;

                            newEdge.source = nextEdge.source;
                            newEdge.next = edge;
                            newEdge.prev = edge.next;

                            edge.next.next = newEdge;
                            edge.next.adjacent_face = newFace;

                            twinEdge = newEdge;
                        }
                        else if (edge.next.next == face.adjacent_halfedge) // Last new Face
                        {
                            nextEdge = edge.next.next;

                            edge.prev = newEdge;

                            newEdge.source = firstvertex;
                            newEdge.next = edge;
                            newEdge.prev = edge.next;

                            edge.next.next = newEdge;
                            edge.next.adjacent_face = newFace;

                            twinEdge.twin = newEdge;
                            newEdge.twin = twinEdge;
                        }
                        else // Other new face
                        {
                            nextEdge = edge.next;

                            newEdge2 = new myHalfedge();
                            this.halfedges.Add(newEdge2);

                            newEdge2.adjacent_face = newFace;
                            newEdge2.source = edge.next.source;
                            newEdge2.next = newEdge;
                            newEdge2.prev = edge;

                            edge.next = newEdge2;
                            edge.prev = newEdge;

                            newEdge.prev = newEdge2;
                            newEdge.next = edge;
                            newEdge.source = firstvertex;

                            twinEdge.twin = newEdge;
                            newEdge.twin = twinEdge;
                            twinEdge = newEdge2;
                        }

                        edge = nextEdge;
                    } while (edge != face.adjacent_halfedge);

                }
        }

        public void copy(myMesh m)
        {
            this.halfedges.Clear();
            this.vertices.Clear();
            this.faces.Clear();

            for (int i = 0; i < m.halfedges.Count; i++ )
            {
                m.halfedges[i].e_id = i;
                this.halfedges.Add(m.halfedges[i].Copy());
            }
            for (int i = 0; i < m.vertices.Count; i++)
            {
                m.vertices[i].v_id = i;
                this.vertices.Add(m.vertices[i].Copy());
            }
            for (int i = 0; i < m.faces.Count; i++)
            {
                m.faces[i].f_id = i;
                this.faces.Add(m.faces[i].Copy());
            }


            myHalfedge mHalfedge;
            foreach (myHalfedge thisHalfedge in this.halfedges)
            {
                mHalfedge = m.halfedges[thisHalfedge.e_id];

                thisHalfedge.source = this.vertices[mHalfedge.source.v_id];
                thisHalfedge.adjacent_face = this.faces[mHalfedge.adjacent_face.f_id];
                thisHalfedge.next = this.halfedges[mHalfedge.next.e_id];
                thisHalfedge.prev = this.halfedges[mHalfedge.prev.e_id];
                if ( mHalfedge.twin != null )
                    thisHalfedge.twin = this.halfedges[mHalfedge.twin.e_id];
            }
            foreach (myVertex thisVertex in this.vertices)
            {
                thisVertex.originof_halfedge = this.halfedges[m.vertices[thisVertex.v_id].originof_halfedge.e_id];
            }
            foreach (myFace thisFace in this.faces)
            {
                thisFace.adjacent_halfedge = this.halfedges[m.faces[thisFace.f_id].adjacent_halfedge.e_id];
            }

        }

        public void subdivisionCatmullClark()
        {
            myMesh newMesh = new myMesh();
            List<myPoint3D> listOfPoint;
            myPoint3D[] tabPoint;
            myHalfedge edge;
            myHalfedge[] tabNewEdge = new myHalfedge[4]; ;
            myVertex centroid;
            myPoint3D Q, R;
            myFace newFace;
            int i = 0 ;
            double twinKey, twinId1, twinId2, n;
            Dictionary<double, myHalfedge> halfedgeTable = new Dictionary<double, myHalfedge>();
            Dictionary<double, int> centroidHalfedgeMake = new Dictionary<double, int>();
            Dictionary<myVertex, List<myPoint3D>> CentroidFacesOfVertex = new Dictionary<myVertex, List<myPoint3D>>(this.vertices.Count);
            Dictionary<myVertex, List<myHalfedge>> EdgeOfVertex = new Dictionary<myVertex, List<myHalfedge>>(this.vertices.Count);
            
            foreach (myFace face in this.faces)
            {
                face.f_id = i;
                i++;

                listOfPoint = new List<myPoint3D>();
                centroid = new myVertex();
                edge = face.adjacent_halfedge;


                do
                {
                    if (!CentroidFacesOfVertex.ContainsKey(edge.source))
                        CentroidFacesOfVertex.Add(edge.source, new List<myPoint3D>());

                    CentroidFacesOfVertex[edge.source].Add(centroid.point);

                    listOfPoint.Add(edge.source.point);
                    edge = edge.next;
                } while (edge != face.adjacent_halfedge);

                centroid.setPoint(myPoint3D.CentroidPoint(listOfPoint.ToArray()));
                centroid.v_id = face.f_id;

                newMesh.vertices.Add(centroid);
            }

            foreach (myVertex vertex in this.vertices)
            {
                vertex.v_id = i;
                i++;

                newMesh.vertices.Add(vertex.Copy());
            }

            foreach (myHalfedge halfedge in this.halfedges)
            {
                twinId1 = halfedge.source.v_id;
                twinId2 = halfedge.target.v_id;
                twinKey = Math.Min(twinId1, twinId2) * this.vertices.Count + Math.Max(twinId1, twinId2);

                if (centroidHalfedgeMake.ContainsKey(twinKey))
                {
                    halfedge.e_id = centroidHalfedgeMake[twinKey];
                }
                else
                {
                    halfedge.e_id = i;
                    i++;

                    tabPoint = new myPoint3D[4];
                    tabPoint[0] = halfedge.source.point;
                    tabPoint[1] = halfedge.target.point;
                    tabPoint[2] = newMesh.vertices[halfedge.adjacent_face.f_id].point;
                    tabPoint[3] = newMesh.vertices[halfedge.twin.adjacent_face.f_id].point;

                    centroid = new myVertex(myPoint3D.CentroidPoint(tabPoint));
                    centroid.v_id = halfedge.e_id;

                    newMesh.vertices.Add(centroid);
                    centroidHalfedgeMake.Add(twinKey, halfedge.e_id);
                }

                if (!EdgeOfVertex.ContainsKey(halfedge.source))
                    EdgeOfVertex.Add(halfedge.source, new List<myHalfedge>());

                EdgeOfVertex[halfedge.source].Add(halfedge);
            }

            foreach (myVertex vertex in this.vertices)
            {
                listOfPoint = new List<myPoint3D>();
                foreach (myHalfedge halfedgeOfVertex in EdgeOfVertex[vertex])
                    listOfPoint.Add(myPoint3D.MidPoint(halfedgeOfVertex.source.point, halfedgeOfVertex.target.point));

                n = EdgeOfVertex[vertex].Count;
                Q = myPoint3D.CentroidPoint(CentroidFacesOfVertex[vertex].ToArray());
                R = myPoint3D.CentroidPoint(listOfPoint.ToArray());

                myPoint3D newPoint = myPoint3D.SumOfPoint((1 / n) * Q, (2 / n) * R, ((n - 3) / n) * vertex.point);

                newMesh.vertices[vertex.v_id].setPoint(newPoint);
            }

            foreach (myHalfedge halfedge in this.halfedges)
            {
                tabNewEdge[0] = new myHalfedge();
                tabNewEdge[1] = new myHalfedge();
                tabNewEdge[2] = new myHalfedge();
                tabNewEdge[3] = new myHalfedge();
                newFace = new myFace();

                newMesh.halfedges.Add(tabNewEdge[0]);
                newMesh.halfedges.Add(tabNewEdge[1]);
                newMesh.halfedges.Add(tabNewEdge[2]);
                newMesh.halfedges.Add(tabNewEdge[3]);
                newMesh.faces.Add(newFace);

                tabNewEdge[0].source = newMesh.vertices[halfedge.e_id];
                tabNewEdge[1].source = newMesh.vertices[halfedge.target.v_id];
                tabNewEdge[2].source = newMesh.vertices[halfedge.next.e_id];
                tabNewEdge[3].source = newMesh.vertices[halfedge.adjacent_face.f_id];

                newFace.adjacent_halfedge = tabNewEdge[0];
                tabNewEdge[0].source.originof_halfedge = tabNewEdge[0];
                tabNewEdge[1].source.originof_halfedge = tabNewEdge[1];
                tabNewEdge[2].source.originof_halfedge = tabNewEdge[2];
                tabNewEdge[3].source.originof_halfedge = tabNewEdge[3];

                tabNewEdge[0].adjacent_face = newFace;
                tabNewEdge[1].adjacent_face = newFace;
                tabNewEdge[2].adjacent_face = newFace;
                tabNewEdge[3].adjacent_face = newFace;

                tabNewEdge[0].next = tabNewEdge[1];
                tabNewEdge[1].next = tabNewEdge[2];
                tabNewEdge[2].next = tabNewEdge[3];
                tabNewEdge[3].next = tabNewEdge[0];

                tabNewEdge[0].prev = tabNewEdge[3];
                tabNewEdge[1].prev = tabNewEdge[0];
                tabNewEdge[2].prev = tabNewEdge[1];
                tabNewEdge[3].prev = tabNewEdge[2];

                foreach( myHalfedge newEdge in tabNewEdge)
                {
                    twinId1 = newEdge.source.v_id;
                    twinId2 = newEdge.target.v_id;
                    twinKey = Math.Min(twinId1, twinId2) * newMesh.vertices.Count + Math.Max(twinId1, twinId2);

                    if (halfedgeTable.ContainsKey(twinKey))
                    {
                        newEdge.twin = halfedgeTable[twinKey];
                        halfedgeTable[twinKey].twin = newEdge;
                    }
                    else
                    {
                        halfedgeTable.Add(twinKey, newEdge);
                    }
                }
            }

            this.copy(newMesh);
        }

        public void check()
        {
            int i, k;
            myVertex v;
            myHalfedge e1, e2;
            myFace f;

            int num_boundaryedges = 0;
            Console.WriteLine("Vertices: " + vertices.Count + ". Halfedges: " + halfedges.Count + ". Faces: " + faces.Count);

            Console.WriteLine("  Checking for null vertices or null originof_halfedges.");
            for (i = 0; i < vertices.Count; i++)
            {
                vertices[i].flag = 0;
                if (vertices[i] == null)
                    Console.WriteLine("\tERROR: vertex " + i + " is null.");
                else if (vertices[i].originof_halfedge == null)
                    Console.WriteLine("\tERROR: vertex " + i + " has originof_halfedge null.");
            }
            Console.WriteLine("  Ended check.\n");


            Console.WriteLine("  Checking prev/next of each halfedge.");
            for (i = 0; i < halfedges.Count; i++)
            {
                halfedges[i].source.flag++;
                if (halfedges[i].source == null)
                    Console.WriteLine("\tERROR: Source is null for edge " + i);
                if (halfedges[i].twin == null) num_boundaryedges++;
                if (halfedges[i].next == null || halfedges[i].prev == null)
                    Console.WriteLine("\tERROR: Next/prev null for edge " + i);
                if (halfedges[i].next.prev != halfedges[i] || halfedges[i].prev.next != halfedges[i])
                    Console.WriteLine("\tERROR: Next/prev not set properly for edge " + i);
                if (halfedges[i].twin != null && halfedges[i] != halfedges[i].twin.twin)
                    Console.WriteLine("\tERROR: Twin pair not set properly for edge " + i);
            }
            if (num_boundaryedges > 0) Console.WriteLine("\tThis mesh has boundary edges.");
            Console.WriteLine("  Ended check.\n");


            Console.WriteLine("  Checking fans of each vertex.");
            for (i = 0; i < vertices.Count; i++)
            {
                v = vertices[i];

                e1 = v.originof_halfedge;
                e2 = e1;

                k = 0;
                do
                {
                    k++;
                    e2 = e2.prev.twin;
                    if (k > 100000) Console.WriteLine("\tERROR: Infinite loop when checking adjacent edges for vertex " + i);
                } while (e2 != null && e2 != e1);
                if (k != v.flag) Console.WriteLine("\tERROR: Fan is not correct for vertex " + i);
            }
            Console.WriteLine("  Ended check.\n");


            Console.WriteLine("  Checking edges of each face.");
            int num_incidentedgesoverallfaces = 0;
            bool istriangular = true;
            for (i = 0; i < faces.Count; i++)
            {
                f = faces[i];

                e1 = f.adjacent_halfedge;
                e2 = e1;

                k = 0;
                do
                {
                    k++;
                    if (e2 == null) Console.WriteLine("\t ERROR: Found null edge on boundary of face " + i);
                    e2 = e2.next;
                    if (k > 100000) Console.WriteLine("\tERROR: Infinite loop when checking adjacent edges for face " + i);
                } while (e2 != e1);
                num_incidentedgesoverallfaces += k;
                if (k > 3) istriangular = false;
            }
            if (num_incidentedgesoverallfaces != halfedges.Count)
                Console.WriteLine("\tERROR: Edges counted over all faces not equal to halfedges.Count.");
            if (!istriangular) Console.WriteLine("\tThis mesh is not triangular.");
            Console.WriteLine("  Ended check.\n");
        }

        public void normalize()
        {
            int i;
            int tmpxmin = 0, tmpymin = 0, tmpzmin = 0, tmpxmax = 0, tmpymax = 0, tmpzmax = 0;

            for (i = 0; i < vertices.Count; i++)
            {
                if (vertices[i].point.X < vertices[tmpxmin].point.X) tmpxmin = i;
                if (vertices[i].point.X > vertices[tmpxmax].point.X) tmpxmax = i;

                if (vertices[i].point.Y < vertices[tmpymin].point.Y) tmpymin = i;
                if (vertices[i].point.Y > vertices[tmpymax].point.Y) tmpymax = i;

                if (vertices[i].point.Z < vertices[tmpzmin].point.Z) tmpzmin = i;
                if (vertices[i].point.Z > vertices[tmpzmax].point.Z) tmpzmax = i;
            }

            double xmin = vertices[tmpxmin].point.X, xmax = vertices[tmpxmax].point.X,
                   ymin = vertices[tmpymin].point.Y, ymax = vertices[tmpymax].point.Y,
                   zmin = vertices[tmpzmin].point.Z, zmax = vertices[tmpzmax].point.Z;

            double scale = Math.Max((xmax - xmin), (ymax - ymin));
            scale = Math.Max(scale, (zmax - zmin));

            for (i = 0; i < vertices.Count; i++)
            {
                vertices[i].point.X -= (xmax + xmin) / 2;
                vertices[i].point.Y -= (ymax + ymin) / 2;
                vertices[i].point.Z -= (zmax + zmin) / 2;

                vertices[i].point.X /= scale;
                vertices[i].point.Y /= scale;
                vertices[i].point.Z /= scale;
            }
        }

        public void computeVertexNormals()
        {
            Dictionary<int, myVector3D> NormalOfFace = new Dictionary<int, myVector3D>(this.faces.Count);
            Dictionary<int, List<int>> FacesOfVertex = new Dictionary<int, List<int>>(this.vertices.Count);
            List<int> listOfFace;
            myVector3D vec;
            myHalfedge edge;

            foreach (myFace _face in this.faces)
            {
                vec = new myVector3D();

                myUtil.computeNormal(_face.adjacent_halfedge.source, _face.adjacent_halfedge.next.source, _face.adjacent_halfedge.next.next.source, vec);
                NormalOfFace.Add(_face.f_id, vec);

                edge = _face.adjacent_halfedge;

                do
                {
                    if (!FacesOfVertex.ContainsKey(edge.source.v_id))
                        FacesOfVertex.Add(edge.source.v_id, new List<int>());

                    FacesOfVertex[edge.source.v_id].Add(_face.f_id);
                    edge = edge.next;
                } while (edge.e_id != _face.adjacent_halfedge.e_id);
            }

            foreach (myVertex _vertex in this.vertices)
            {
                vec = new myVector3D();

                if (FacesOfVertex.ContainsKey(_vertex.v_id))
                {
                    listOfFace = FacesOfVertex[_vertex.v_id];

                    foreach (int id_f in listOfFace)
                        vec = vec + NormalOfFace[id_f];

                    if (listOfFace.Count != 0)
                        vec = vec / listOfFace.Count;
                }

                _vertex.normal = vec;
            }
        }    

        public void computeSilhouette(myPoint3D c, myVector3D v)
        {
            myVector3D normal_face1, normal_face2, vectorToCamera_face;

            foreach (myHalfedge edge in halfedges)
            {
                normal_face1 = edge.adjacent_face.GetNormal();
                normal_face2 = edge.twin.adjacent_face.GetNormal();

                vectorToCamera_face = c - edge.source.point;

                if (myUtil.computeDotproduct(normal_face1, vectorToCamera_face) >= 0 ^ myUtil.computeDotproduct(normal_face2, vectorToCamera_face) >= 0)
                    edge.flag = 1;
                else
                    edge.flag = 0;
            }
        }

        void computeParticularBoundary(myPoint3D c, double theta)
        {
            myVector3D normal_face1, normal_face2, vectorToCamera_face1, vectorToCamera_face2;
            myVertex vertexOn_face;
            double dot1, dot2;


            foreach (myHalfedge edge in halfedges)
            {
                normal_face1 = edge.adjacent_face.GetNormal();
                normal_face2 = edge.twin.adjacent_face.GetNormal();

                vertexOn_face = edge.source;

                vectorToCamera_face1 = myVector3D.CreateVector(vertexOn_face.point, c);
                vectorToCamera_face2 = myVector3D.CreateVector(vertexOn_face.point, c);

                dot1 = myUtil.computeDotproduct(normal_face1, vectorToCamera_face1);
                dot2 = myUtil.computeDotproduct(normal_face2, vectorToCamera_face2);

                edge.flag = (dot1 * dot2 < 0) ? 1 : 0;

                if (theta < 0) continue;

                double angle = myVector3D.GetAngle(normal_face1, normal_face2);

                edge.flag = (angle <= theta) ? ((edge.flag == 1) ? 3 : 2) : edge.flag;
            }
        }

        //http://maverick.inria.fr/~Nicolas.Holzschuch/cours/class7_qslim.pdf
        //http://classes.soe.ucsc.edu/cmps160/Spring05/finalpages/scyiu/
        //http://www.esiee.fr/~mustafan/IN4M12-13/COURS/5-s97-quadric-talk.pdf
        public void computeSimplification(int numvert)
        {
            Dictionary<myVertex, List<Matrix4>> KOfVertex = new Dictionary<myVertex, List<Matrix4>>(this.vertices.Count);
            Dictionary<myVertex, Matrix4> Q = new Dictionary<myVertex, Matrix4>(this.vertices.Count);          
            myPlane3D plane;
            myHalfedge edge, edgeToContract;
            myVertex vertex;
            myFace face;
            Matrix4 K, newQ;
            int nbVertice = this.vertices.Count - numvert;

            for (int i = 0; i < this.faces.Count; i++)
            {
                face = this.faces[i];
                face.f_id = i;
                
                plane = new myPlane3D();
                myUtil.computePlaneOfFace(face, plane);
                K = plane.GetQuadric();

                edge = face.adjacent_halfedge;

                do
                {
                    if (!KOfVertex.ContainsKey(edge.source))
                        KOfVertex.Add(edge.source, new List<Matrix4>());

                    KOfVertex[edge.source].Add(K);

                    edge = edge.next;
                } while (edge != face.adjacent_halfedge);
            }

            for (int i = 0; i < this.vertices.Count; i++)
            {
                vertex = this.vertices[i];
                vertex.v_id = i;

                Q.Add(vertex, Matrix4.MakeSum(KOfVertex[vertex]));
            }

            while (this.vertices.Count > nbVertice)
            {
                edgeToContract = getMinError(Q);
                if (edgeToContract == null)
                    break;

                newQ = Q[edgeToContract.source] + Q[edgeToContract.target];;
                Q[edgeToContract.source] = newQ;
                Q.Remove(edgeToContract.target);
                edgeContract(edgeToContract);
            }

        }

        public myHalfedge getMinError(Dictionary<myVertex, Matrix4> Q)
        {
            myHalfedge minEdge = null;
            double minError = double.MaxValue, error;
            Matrix4 Qedge;
            double x,y,z;


            foreach (myHalfedge edge in this.halfedges)
            {
                x = (edge.source.point.X + edge.target.point.X) / 2;
                y = (edge.source.point.Y + edge.target.point.Y) / 2;
                z = (edge.source.point.Z + edge.target.point.Z) / 2;

                if (!Q.ContainsKey(edge.source) || !Q.ContainsKey(edge.target))
                    continue;

                Qedge = Q[edge.source] + Q[edge.target];

                error = x * x * Qedge.matrix[0, 0] + 2 * x * y * Qedge.matrix[0, 1] + 2 * x * z * Qedge.matrix[0, 2]
                        + 2 * x * Qedge.matrix[0, 3] + y * y * Qedge.matrix[1, 1] + 2 * y * z * Qedge.matrix[1, 2]
                        + 2 * y * Qedge.matrix[1, 3] + z * z * Qedge.matrix[2, 2] + 2 * z * Qedge.matrix[2, 3]
                        + Qedge.matrix[3, 3];

                if (error < minError)
                {
                    minError = error;
                    minEdge = edge;
                }
            }    

            return minEdge;
        }

        public void edgeContract(myHalfedge e)
        {
            List<myHalfedge> edgeToDelete = new List<myHalfedge>();
            myHalfedge edge;

            edge = e;
            do
            {
                edgeToDelete.Add(edge);
                edge = edge.next;
            } while (edge != e);

            edge = e.twin;
            do
            {
                edgeToDelete.Add(edge);
                edge = edge.next;
            } while (edge != e.twin);

            e.source.point = myPoint3D.MidPoint(e.source.point, e.target.point);

            this.faces.Remove(e.adjacent_face);
            this.faces.Remove(e.twin.adjacent_face);

            this.vertices.Remove(e.target);

            foreach (myHalfedge _edge in edgeToDelete)
                this.halfedges.Remove(_edge);

            foreach (myVertex vertex in this.vertices)
                if (edgeToDelete.Contains(vertex.originof_halfedge))
                    //vertex.originof_halfedge = vertex.originof_halfedge.prev.twin; //Error
                    foreach (myHalfedge _edge in this.halfedges)
                        if (_edge.source == vertex)
                        {
                            vertex.originof_halfedge = _edge;
                            break;
                        }

            foreach (myHalfedge _edge in this.halfedges)
                if (_edge.source == e.target)
                    _edge.source = e.source;

            e.prev.twin.twin = e.next.twin;
            e.next.twin.twin = e.prev.twin;

            e.twin.prev.twin.twin = e.twin.next.twin;
            e.twin.next.twin.twin = e.twin.prev.twin;
        }
        
    }

}
